/*
 * @Author: huangyupei huangyupei2021@ia.ac.cn
 * @Date: 2024-09-20 11:03:15
 * @LastEditors: huangyupei huangyupei2021@ia.ac.cn
 * @LastEditTime: 2025-03-25 20:13:13
 * @FilePath: /SelfCalib_OptiAcoustic/src/readImageFeatures.cc
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "readImageFeatures.h"

using namespace std;

int readCameraFeaturesFromTXT(const string filename, vector<cv::KeyPoint>& keypoints) {
    // 打开文件
    std::ifstream file(filename);
    if (!file.is_open()) {
        std::cerr << "无法打开文件!" << std::endl;
        cout << "filename: " << filename << endl;
        return -1;
    }

    // 设置随机数生成器
    std::random_device rd;  // 随机种子
    std::mt19937 gen(rd()); // 使用Mersenne Twister引擎
    std::normal_distribution<double> dist(0.0, 1.0); // 零均值，标准差为 1 pixel

    // 读取文件中的特征点
    int index;
    float x, y;
    while (file >> index >> x >> y) {
        // 创建一个KeyPoint对象，并设置坐标
        x += dist(gen);
        y += dist(gen);
        cv::KeyPoint kp;
        kp.pt = cv::Point2f(x, y); // 设置x, y坐标
        kp.size = 1.0f;            // 设置size（此处可以根据实际需要调整）
        kp.angle = -1;             // 默认值，表示没有角度信息

        // 将KeyPoint添加到vector中
        keypoints.push_back(kp);
    }

    // 关闭文件
    file.close();

    return 0;
}

vector<Eigen::Vector2d> getSonarMeasXY(const string sonar_filename) {
    vector<Eigen::Vector2d> vSonarMeas;
    assert(readSonarFeaturesFromTXT(sonar_filename, vSonarMeas) != -1);

    // 设置随机数生成器
    std::random_device rd;  // 随机种子
    std::mt19937 gen(rd()); // 使用Mersenne Twister引擎
    std::normal_distribution<double> dist(0.0, 0.005); // 零均值，标准差为 xxx m

    // 下面将测距、方位角的测量转变为xs ys的测量
    int num_acoustic_points = vSonarMeas.size();
    for (int i= 0; i < num_acoustic_points; i++) {
        double r = vSonarMeas[i][0], theta = vSonarMeas[i][1];        // 角度的单位为rad
        // 添加噪声 
        vSonarMeas[i][0] = r * sin(theta) + dist(gen);
        vSonarMeas[i][1] = r * cos(theta) + dist(gen);
    }

    return vSonarMeas;
}

int readSonarFeaturesFromTXT(const string filename, vector<Eigen::Vector2d>& vSonarMeas) {
    // 打开文件
    std::ifstream file(filename);
    if (!file.is_open()) {
        std::cerr << "无法打开文件!" << std::endl;
        cout << "filename: " << filename << endl;
        return -1;
    }
    // 读取文件中的特征点
    int index;
    double r, theta;
    while (file >> index >> r >> theta) {
        Eigen::Vector2d sonar_meas(r, theta);
        // 将KeyPoint添加到vector中
        vSonarMeas.push_back(sonar_meas);
    }
    // 关闭文件
    file.close();

    return 0;
}




